#include <iostream>
#include <cmath>

class KalmanFilter
{
public:
    KalmanFilter(double process_noise_var, double measurement_noise_var,
                 double est_error_var, double initial_state)
        : process_noise_var_(process_noise_var),
          measurement_noise_var_(measurement_noise_var),
          est_error_var_(est_error_var),
          state_estimate_(initial_state),
          error_estimate_squared_(est_error_var) {}

    void predict()
    {
        // 预测步骤: 使用上一时刻的状态估计和过程噪声预测下一时刻的状态
        state_estimate_ += 0; // 假设状态转移函数为恒定（无控制输入的情况）
        error_estimate_squared_ += process_noise_var_;
    }

    void update(double measurement)
    {
        // 更新步骤: 利用测量值更新状态估计和误差估计
        const double residual = measurement - state_estimate_;
        const double residual_variance = error_estimate_squared_ + measurement_noise_var_;

        // 计算卡尔曼增益
        const double kalman_gain = error_estimate_squared_ / residual_variance;

        // 更新状态估计和误差估计
        state_estimate_ += kalman_gain * residual;
        error_estimate_squared_ = (1 - kalman_gain) * error_estimate_squared_;
    }

    double getStateEstimate() const
    {
        return state_estimate_;
    }

private:
    double process_noise_var_;      // 过程噪声方差
    double measurement_noise_var_;  // 测量噪声方差
    double est_error_var_;          // 状态估计误差方差
    double state_estimate_;         // 状态估计值
    double error_estimate_squared_; // 状态估计误差的平方
};

int main()
{
    // 初始化卡尔曼滤波器参数
    double process_noise_var = 1e-2;     // 过程噪声方差
    double measurement_noise_var = 1e-1; // 测量噪声方差
    double est_error_var = 1.0;          // 初始估计误差方差
    double initial_state = 0.0;          // 初始状态估计

    // 创建卡尔曼滤波器对象
    KalmanFilter kf(process_noise_var, measurement_noise_var, est_error_var, initial_state);

    float num_arr[] = {1, 1.05, 1.1, 1, 2, 1.1, 0.89, 1.2, 1, 0.77, 1, 0.77};
    int n = sizeof(num_arr) / sizeof(float);
    // 模拟测量数据并进行滤波处理
    for (int i = 0; i < n; ++i)
    {
        // 假设的真实状态（这里简化处理，实际情况中这是未知的）
        // double true_state = i + (rand() % 100) / 100.0 - 0.5; // 加入一些随机扰动
        // 测量值（真实状态加上测量噪声）
        double measurement = num_arr[n];

        // 执行预测和更新步骤
        kf.predict();
        kf.update(measurement);

        // 输出当前的估计状态
        std::cout << "Estimated State: " << kf.getStateEstimate()
                //   << ", True State: " << true_state
                  << ", Measurement: " << measurement << std::endl;
    }

    return 0;
}